#include"visual_odom/config.h"

namespace test_visual_odom{

Config::Config(ros::NodeHandle &n, const string param_name){
    string config_file;
    config_file = rosReadParam<string>(n,param_name);
    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        std::cerr << "ERROR: Wrong path to settings" << std::endl;
    }

    fsSettings["left_image_topic"]>>left_image_topic_;
    fsSettings["right_image_topic"]>>right_image_topic_;
    
    fsSettings["model_type"]>>this->camera_model_type_;
    this->image_width_ = fsSettings["image_width"];
    this->image_height_ = fsSettings["image_height"];

    cv::FileNode fn = fsSettings["camera_left_distortion_parameters"];
    camera_left_k1_ =static_cast<double>(fn["k1"]);
    camera_left_k2_ =static_cast<double>(fn["k2"]);
    camera_left_p1_ =static_cast<double>(fn["p1"]);
    camera_left_p2_ =static_cast<double>(fn["p2"]);
    fn = fsSettings["camera_left_projection_parameters"];
    camera_left_fx_ = static_cast<double>(fn["fx"]);
    camera_left_fy_ = static_cast<double>(fn["fy"]);
    camera_left_cx_ = static_cast<double>(fn["cx"]);
    camera_left_cy_ = static_cast<double>(fn["cy"]);

    fn = fsSettings["camera_right_distortion_parameters"];
    camera_right_k1_ =static_cast<double>(fn["k1"]);
    camera_right_k2_ =static_cast<double>(fn["k2"]);
    camera_right_p1_ =static_cast<double>(fn["p1"]);
    camera_right_p2_ =static_cast<double>(fn["p2"]);
    fn = fsSettings["camera_left_projection_parameters"];
    camera_right_fx_ = static_cast<double>(fn["fx"]);
    camera_right_fy_ = static_cast<double>(fn["fy"]);
    camera_right_cx_ = static_cast<double>(fn["cx"]);
    camera_right_cy_ = static_cast<double>(fn["cy"]);

    fsSettings["extrinsicRotation"] >> R_camera_left2camera_right_;
    fsSettings["extrinsicTranslation"] >> t_camera_left2camera_right_;

    num_features_ = fsSettings["num_features"];
    num_features_init_ = fsSettings["num_features_init"];
    num_features_tracking_ = fsSettings["num_features_tracking"];

    fsSettings.release();
}

template<typename T>
T Config::rosReadParam(ros::NodeHandle &n, string name){
    T ans;
    if (n.getParam(name, ans))
    {
        ROS_INFO_STREAM("Loaded " << name << ": " << ans);
    }
    else
    {
        ROS_ERROR_STREAM("Failed to load " << name);
        n.shutdown();
    }
    return ans;
}

}